#include "parser.h"

Parser::Parser()
{
    Parser::init();

    objectball=false;
    goal=false;
    play_on=false;
    drop_ball=false;
    foul=false;
    nearertoball=true;

    goal_kick=false;
    kick_off=false;
    free_kick=false;
    free_kick_foul=false;
    before_kick_off=false;
    corner_kick=false;
    offside=false;
    half_time=false;
    kick_in=false;
    goalie_catch_ball=false;

    team_name="";

    searching=false;
}

void Parser::init(){
    for(int i=0; i<6; i++)
    {
        ball[i] = 0;

        line_r[i] = 0;
        line_l[i] = 0;

        line_b[i]=0;
        line_t[i]=0;


        flag_c[i]=0;
        flag_c_t [i]=0;
        flag_c_b [i]=0;

        flag_r_t [i]=0;
        flag_r_b [i]=0;

        flag_g_l_t[i]=0;
        goal_r[i]=0;
        flag_g_l_b[i]=0;

        flag_g_r_t[i]=0;
        goal_l[i]=0;
        flag_g_r_b[i]=0;

        flag_p_l_c[i]=0;
        flag_p_r_c[i]=0;


        mygoal[i]=0;
        theirgoal[i]=0;

        p_id[0]=0;
        player[i]=0;
    }

    team_name=" ";
    distPlayers.clear();
    objectball=false;
}


void Parser:: parse (string &data)
{

    //cout << data << endl;

    hear(data, "(hear ");

    // time=findint(data, "(hear ");


    if(strcmp(data.c_str(), "(see ")){ //se ejecuta siempre

        findfloats(data, "(line r) ", line_r);
        findfloats(data, "(line l) ", line_l);

        findfloats(data, "(line b) ", line_b);
        findfloats(data, "(line t) ", line_t);

        findfloats(data, "(goal r) ", goal_r);
        findfloats(data, "(goal l) ", goal_l);

        findfloats(data, "(flag c) ", flag_c);
        findfloats(data, "(flag c t) ", flag_c_t);
        findfloats(data, "(flag c b) ", flag_c_b);
        findfloats(data, "(flag r t) ", flag_r_t);
        findfloats(data, "(flag r b) ", flag_r_b);
        findfloats(data, "(flag g l t) ", flag_g_l_t);
        findfloats(data, "(flag g l b) ", flag_g_l_b);
        findfloats(data, "(flag g r t) ", flag_g_r_t);

        findfloats(data, "(flag p l c) ", flag_p_l_c);
        findfloats(data, "(flag p r c) ", flag_p_r_c);

        findint2(data, "(stamina ", stamina, effort);

        findfloats(data, "(speed ", speed);

        time=findint(data, "(see ");


        findfloats(data, "(ball) ", ball);  //gets the ball's pos
        findplayer(data, p_id, team_name, player);

        /*************************nearer to ball implementation************************/

        float ballx, bally, playerx, playery, p_b_x, p_b_y;
        vector <float> p_b_dist;


        ballx=ball[0]*cos(ball[1]);
        bally=ball[0]*sin(ball[1]);

        if(distPlayers.size()>0)
        {
            for (int i=0; i<distPlayers.size(); i++)
            {
                playerx=distPlayers[i]*cos(dirPlayers[i]);
                playery=distPlayers[i]*sin(dirPlayers[i]);

                p_b_x=ballx-playerx;
                p_b_y=bally-playery;

                p_b_dist.push_back(sqrt(pow(p_b_x, 2)+pow(p_b_y, 2)));
            }

            sort(p_b_dist.begin(), p_b_dist.end());

            if((p_b_dist[0]>=ball[0]) ){ //&& ball[0]<20
                nearertoball=true;
                //cout<<"I'm nearer!!"<<endl;
            }
            else
                nearertoball=false;
        }

        //    //***************************************************************************+
        //        if(distPlayers.size()>0)
        //        { //Si va en primera fila no ve a los de detrás.
        //            if(distPlayers[distPlayers.size()-1]>ball[0])
        //                nearertoball=true;
        //            else
        //                nearertoball=false;
        //        }

    }
    /**********************end nearer to ball implementation ***********************/

    if(strcmp(data.c_str(),"(body_sensor ")) //se ejecuta siempre
    {
        head_angle=findint(data, "head_angle");

        counters.push_back(findint(data, "kick"));
        counters.push_back(findint(data, "dash"));
        counters.push_back(findint(data, "turn"));
        counters.push_back(findint(data, "say"));
        counters.push_back(findint(data, "turn_neck"));
        counters.push_back(findint(data, "catch"));
        counters.push_back(findint(data, "move"));
        counters.push_back(findint(data, "change_view"));

        time=findint(data, "(body_sensor ");
    }


    if(field=="right"){
        for (int i=0; i<6; i++){
            mygoal[i]=goal_r[i];
            theirgoal[i]=goal_l[i];
        }
    }
    if(field=="left"){
        for (int i=0; i<6; i++){
            mygoal[i]=goal_l[i];
            theirgoal[i]=goal_r[i];
        }
    }

    // if( (flag_l_t[0]<2 &&flag_l_t[0]>0)||(flag_r_t[0]<2 && flag_r_t[0]>0) ||(flag_l_b[0]<2 && flag_l_b[0]>0) ||(flag_r_b[0]<2 && flag_r_b[0]>0) )
    //init();

    //    cout << data << endl;
    //    cout << "Time: "<<time<<endl;
    //    cout << "Arg: " << ball[0] << "  Ang: "<<ball[1] << endl;
    //    cout << "Speed arg: " << speed[0] << "  Speed ang: " << speed[1] << endl;
    //    cout << "Stamina: " << stamina << "  Effort: " << effort << endl;
    //    cout << "Line r: " << line_r[0] << " Line l: " << line_l[0] << endl;
    //    cout << "Flag c: " << flag_c[0] <<  "  Flag c t: " << flag_c_t[0] << " Flag c b: " << flag_c_b[0] << endl;
    //    cout << "Flag g l t: " << flag_g_l_t[0] << " Flag g l b: " << flag_g_l_b[0] << endl;
    //    cout << "Flag g r t: " << flag_g_r_t[0] << " Flag g r b: " << flag_g_r_b[0] << endl;
    //    cout << "Goal r: " << goal_r[0]<<" " <<goal_r[1] << " Goal l: " << goal_l[0]<<" "<<goal_l[1]<< endl;
    //    cout << "Mygoal: " << mygoal[0]<<" " <<mygoal[1] << " Theirgoal: " << theirgoal[0]<<" "<<theirgoal[1]<< endl;
    //    cout << "Pid: " <<p_id[0] <<  "   Team:  " <<team_name << "  PLayer pos: " << player[0] << "  "<< player[1] << endl;
}


void Parser:: findfloats(string &str, const string name, float see_info[])
{
    size_t found =0, middle=0, final=0;
    int j=0, k=0;
    string Distance, Direction;// DistChange, DirChange, BodyFacingDir, HeadFacingDir;

    found=str.find(name);

    if (strcmp(str.c_str(), name.c_str()) && found!= string::npos)
    {

        if (name=="(ball) ")
            objectball=true;

        Direction.clear();
        Distance.clear();

        found+=name.length();
        middle=str.find(" ", found);
        final=str.find(")", middle);

        for (size_t i= found; i <= final; i++)
        {
            if (i<middle){
                Distance[j]=str.at(i);
                j++;
            }
            if (i==middle)
            {
                Distance[j]= '\0';
                see_info[0]= atof(Distance.c_str());

            }

            if (i>middle && i<final){
                Direction[k]=str.at(i);
                k++;
            }

            if (i==final)
            {
                Direction[k]= '\0';
                see_info[1]= atof(Direction.c_str());
            }
        }
    }

}


void Parser:: findplayer(string &str, float id[], string &team_name, float see_info[])
{
    int a=-1,b=-1,c=-1;
    size_t found=0, found2=0, found3=0, foundy=0;
    distPlayers.clear();
    dirPlayers.clear();

    //Probamos las distintas configuraciones
    string name="(player ";
    string name2="(player) "; //de mi equipo
    string name3= "(Player) "; //del otro equipo


    while (found!=string::npos)
    {
        found=str.find(name,found+1);
        a++;
    }
    found=str.find(name,0);

    while (found2!=string::npos)
    {
        found2=str.find(name2,found2+1);
        b++;
    }
    found2=str.find(name2,0);

    while (found3!=string::npos)
    {
        found3=str.find(name3,found3+1);
        c++;
    }
    found3=str.find(name3,0);

    //cout << "abc: " << a << " " << b << " " << c << endl;

    for (int w=0; w<(a+b+c); w++)
    {
        bool our_player=false;

        size_t id_t=0, n1=0, n2=0,n3=0,n4=0, final=0;
        int j=0, k=0, l=0, m=0;
        string id_number, team, Distance, Direction, DistChange, DirChange;// BodyFacingDir, HeadFacingDir;

        if (found!= string::npos)
        {
            id_number.clear();
            Direction.clear();
            Distance.clear();
            DistChange.clear();
            DirChange.clear();
            team.clear();

            found+=name.length();
            id_t=str.find(" ", found);
            n1=str.find(") ", id_t);
            n2=str.find(" ", n1+2);
            n3=str.find(" ", n2+1);
            n4=str.find(" ", n3+1);
            final=str.find(")", n4+1);

            // cout << "posiciones: Found:" << found << " id: " << id_t << " n1:  " << n1 << " n2:  "<< n2 <<" n3:  "<<n3 << " n4:  " <<n4 << " final:  " << final<< endl;

            for (size_t i= found; i <= final; i++)
            {
                //team name
                if ((found<=i)&&(i<id_t))
                {
                    team[j]=str.at(i);
                    j++;
                }
                else if (i==id_t)
                {
                    team[j]= '\0';
                    team_name= team.c_str();
                }

                //id number
                else if ((id_t<i)&&(i<n1))
                {
                    id_number[k]=str.at(i);
                    k++;
                }

                else if (i==n1)
                {
                    id_number[k]='\0';
                    id[0] = atof(id_number.c_str());
                }

                //Distance
                else  if ((n1<i)&&(i<n2))
                {
                    Distance[l]=str.at(i);
                    l++;
                }

                else if (i==n2)
                {
                    Distance[l]= '\0';
                    see_info[0]= atoi(Distance.c_str());
                }

                //Direction
                else if ((n2<i)&&(i<n3))
                {
                    Direction[m]=str.at(i);
                    m++;
                }

                else if (i==n3)
                {
                    Direction[m]= '\0';
                    see_info[1]= atoi(Direction.c_str());
                }
            }distPlayers.push_back(see_info[0]);
            dirPlayers.push_back(see_info[1]);
        }

        else if ((found2!= string::npos)||(found3!= string::npos))
        {
            if (b>0)//found2
            {
                our_player=true;
                found2+=name2.length();// name2 = name3 en longitud
                n1=str.find(" ", found2);
                n2=str.find(")", n1);
                foundy=found2;
            }

            else if (c>0)//found3
            {
                found3+=name3.length();
                n1=str.find(" ", found3);
                n2=str.find(")", n1);
                foundy=found3;
            }

            Direction.clear();
            Distance.clear();



            for (size_t i= foundy; i <= n2; i++)
            {
                //Distance
                if ((foundy<=i)&&(i<n1))
                {
                    Distance[l]=str.at(i);
                    l++;
                }

                if (i==n1)
                {
                    Distance[l]= '\0';
                    see_info[0]= atoi(Distance.c_str());
                }

                //Direction
                if ((n1<i)&&(i<n2))
                {
                    Direction[m]=str.at(i);
                    m++;
                }

                if (i==n2)
                {
                    Direction[m]= '\0';
                    see_info[1]= atoi(Direction.c_str());
                }
            }distPlayers.push_back(see_info[0]);
            dirPlayers.push_back(see_info[1]);
        }

        //        if (our_player==true)
        //            cout << "\n Visto jugador (" << w+1 << " de " << a+b+c << ") a " << see_info[0] << " metros."<< endl;
        //        else
        //            cout << "\n Visto jugador (del otro equipo) (" << w+1 << " de " << a+b+c << ") a " << see_info[0] << " metros."<< endl;

    }

    //    if(distPlayers.size()>=1)
    //    {
    //        cout << "Jugadores a: " << endl;

    //        for(int t=0; t<distPlayers.size(); t++)
    //        {
    //            cout << distPlayers.at(t)<<"m ";
    //        }
    //    }

}


int  Parser:: findint(string &str, const string name)
{
    size_t found=0;
    int j=0;
    string solution, lastsolution;

    found=str.find(name);

    if (found!= string::npos)
    {
        for (size_t i= found+name.length(); i < str.length(); i++)
        {
            if (&str[i]==" ")
                break;
            solution[j]=str.at(i);
            j++;
        }
    }
    else
        solution=lastsolution;

    lastsolution=solution;
    return atoi(solution.c_str());
}


void  Parser:: findint2(string &str, const string name,  int &value1,  int &value2)
{
    size_t found=0;
    int j=0, n=0;
    string solution=" ";

    found=str.find(name);

    if (found!= string::npos)
    {
        for (size_t i= found+name.length(); i < str.length(); i++)
        {
            if (str.at(i) == ' ')
                break;
            solution[j]=str.at(i);
            j++;
            n=(int)i;
            if (i==str.length()-1){
                solution[j]='\0';
            }
        }

        value1= atoi(solution.c_str());
        j=0;
        n=n+2;

        if (str.at(n)!= ')')
        {
            solution=" ";

            for (size_t k= n; k < str.length(); k++)
            {
                if (str.at(k) == ')')
                    break;
                solution[j]=str.at(k);
                j++;
                if (k==str.length()-1)
                    solution[j]='\0';

            }
            value2= atoi(solution.c_str());
        }
    }
}



void  Parser:: findint2(string &str, const string name, int value[]) //sobrecarga
{
    size_t found=0;
    int j=0, n=0;
    string solution=" ";

    found=str.find(name);

    if (found!= string::npos)
    {
        for (size_t i= found+name.length(); i < str.length(); i++)
        {
            if (str.at(i) == ' ')
                break;
            solution[j]=str.at(i);
            j++;
            n=(int)i;
        }

        value[0]= atoi(solution.c_str());
        j=0;
        n=n+2;

        if (str.at(n)!= ')')
        {
            solution=" ";

            for (size_t k= n; k < str.length(); k++)
            {
                if (str.at(k) == ')')
                    break;
                solution[j]=str.at(k);
                j++;

            }
            value[1]= atoi(solution.c_str());
        }
    }
}



string  Parser:: findstring(string &str, const string name)
{
    size_t found;
    int j=0;
    string solution;

    if (str.compare(name)){
        found=str.find(name);
        for (size_t i= found+name.length(); i < str.length(); i++)
        {
            if (&str[i]==" ")
                break;
            solution[j++]=str.at(i);
        }
        return (solution);
    }
}


void Parser:: hear(string &str, const string name)
{
    int found_name, found_drop, found_play_on, found_goal, found_foul, found_kick_off, found_free_kick, found_goal_kick;
    int found_corner_kick, found_offside, found_goalie_catch, found_before_kick_off, found_free_kick_foul;
    int found_half_time, found_kick_in;

    found_name=str.find(name);

    if(found_name!=string::npos)
    {
        cout<< "\n Oído cocina: " << str << endl;

        found_drop=str.find("drop_ball");
        found_play_on=str.find("play_on");
        found_goal=str.find(" goal_");
        found_foul=str.find(" foul_");
        found_kick_off=str.find(" kick_off_");
        found_free_kick=str.find(" free_kick_");
        found_corner_kick=str.find(" corner_kick_");
        found_offside=str.find(" offside_");
        found_half_time=str.find(" half_time");
        found_kick_in=str.find(" kick_in_");
        found_goalie_catch=str.find(" goalie_catch_ball_");
        found_free_kick_foul=str.find(" free_kick_foul_");
        found_before_kick_off=str.find(" before_kick_off");
        found_goal_kick=str.find("referee goal_kick_");

        if(found_drop!= string::npos)
        {
            cout << "DROP_BALL: La pelota ha cambiado de posición. Búscala!"<< endl;
            drop_ball=1;
            init();
        }

        else if(found_play_on!= string::npos)
        {
            cout << "PLAY_ON: Se reanuda el juego."<< endl;
            play_on=1;

            goal=0;
            drop_ball=0;
            goalie_catch_ball=0;

            before_kick_off=0;
            kick_off=0;
            half_time=0;
            kick_in=0;
            init();

        }

        else if(found_goal!= string::npos)
        {
            cout << "GOAL: Han marcado gol. Volver a posición inicial"<< endl;
            goal=1;
            init();

        }

        else if(found_foul!= string::npos)
        {
            cout << "FOUL: falta!!! ¿Free kick?" << endl;
            foul=1;
            init();
        }

        else if(found_kick_off != string::npos)
        {
            cout << "KICK_OFF: Inicio " << endl;
            kick_in=1;
            goal_kick=0;
            before_kick_off=0;
            init();
        }

        else if(found_free_kick != string::npos)
        {
            cout << "FREE_KICK: Tiro libre (sacar una falta) " << endl;
            kick_in=1;
            init();
        }

        else if(found_corner_kick != string::npos)
        {
            cout << "CORNER_KICK: Corner" << endl;
            kick_in=1;
            init();
        }

        else if(found_offside != string::npos)
        {
            cout << "OFFSIDE: Fuera de juego" << endl;
            offside=1;
            init();
        }

        else if(found_half_time != string::npos)
        {
            cout << "HALF TIME: Descanso" << endl;
            half_time=1;
            init();
        }

        else if(found_kick_in != string::npos)
        {
            cout << "KICK IN: Saque de banda" << endl;
            kick_in=1;
            before_kick_off=0;
            init();
        }

        else if(found_goalie_catch!= string::npos)
        {
            cout << "PORTERO COGE PELOTA: lanzarla lejos"<< endl;
            goalie_catch_ball=1;
            init();
        }

        else if(found_free_kick_foul != string::npos)
        {
            cout << "FREE_KICK_FOUL: Tiro libre (sacar una falta) " << endl;
            kick_in=1;
            init();
        }

        else if(before_kick_off!= string::npos)
        {
            cout << "BEFORE KICK OFF: PREPARADOS " << endl;
            before_kick_off=1;
            init();
        }

        else if(goal_kick!= string::npos)
        {
            cout << "GOAL KICK: Portero saca " << endl;
            goal_kick=1;
            init();
        }
    }
}


/******************************* FILE PARSER *************************************/


void Parser:: getInitPos(vector<int> &ret, unsigned short id, string path)
{
    ret.clear();
    ifstream configfile(path.c_str());
    int x,y;
    stringstream coord;
    coord.str("");

    coord<<"player"<<id<<" ";
    string dummy;

    while(getline (configfile, dummy))
    {
        dummy+="\0";
        Parser::findint2(dummy, coord.str(), x,y);
    }

    vector<int>returning;
    returning.push_back(x);
    returning.push_back(y);
    ret=returning;
    //cout <<coord.str()<<" "<<ret[0]<<" " <<ret[1]<<endl;
}

